#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
sys.path.append("..")
sys.path.append("../../")
from lib.BwRobotLib import BwRobotLib
from lib.MiniBotControlLib import MiniBotControlLib
from RobotManipulator.ManipulatorRobot import ManipulatorRobot
from RobotManipulator.ManipulatorActions import ManipulatorActions
import keyboard
import time

robotlib = BwRobotLib()
manipulator = ManipulatorRobot(robotlib)
#robotlib.mclib.SetIsDebug(True)

#连接机器人设备
robotlib.connectRobot("192.168.3.46",55000)
#robotlib.connectRobot("10.10.100.254",55000)

#加载配置（兼容VS项目启动方式及文件直接启动方式）
import os,inspect
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"
robotlib.loadConfig(cfile)

#配置操作臂
manipulator.bindIds(robotlib.module_ids)

#-----------------------------------------------------------------------------
#操作臂
#-----------------------------------------------------------------------------

#读取当前坐标
manipulator.readAngles()

#控制空间坐标
def controlCoordinate(key):
    cmd = key.name
    #print(cmd)
    #X
    if(cmd == 'i'):
        val = manipulator.spatial_coordinate[0] + 1
        manipulator.spatial_coordinate[0] = val
    if(cmd == 'k'):
        val = manipulator.spatial_coordinate[0] - 1
        manipulator.spatial_coordinate[0] = val

    #Y
    if(cmd == 'o'):
        val = manipulator.spatial_coordinate[1] + 1
        manipulator.spatial_coordinate[1] = val
    if(cmd == 'l'):
        val = manipulator.spatial_coordinate[1] - 1
        manipulator.spatial_coordinate[1] = val

    #Z
    if(cmd == 'p'):
        val = manipulator.spatial_coordinate[2] + 1
        manipulator.spatial_coordinate[2] = val
    if(cmd == 'm'):
        val = manipulator.spatial_coordinate[2] - 1
        manipulator.spatial_coordinate[2] = val

    #Roll
    if(cmd == 'z'):
        val = manipulator.spatial_coordinate[3] + 1
        manipulator.spatial_coordinate[3] = val
    if(cmd == 'x'):
        val = manipulator.spatial_coordinate[3] - 1
        manipulator.spatial_coordinate[3] = val

    #Pitch
    if(cmd == 'c'):
        val = manipulator.spatial_coordinate[4] + 1
        manipulator.spatial_coordinate[4] = val
    if(cmd == 'v'):
        val = manipulator.spatial_coordinate[4] - 1
        manipulator.spatial_coordinate[4] = val

     #Yaw
    if(cmd == 'b'):
        val = manipulator.spatial_coordinate[5] + 1
        manipulator.spatial_coordinate[5] = val
    if(cmd == 'n'):
        val = manipulator.spatial_coordinate[5] - 1
        manipulator.spatial_coordinate[5] = val

    manipulator.pushSpatialCoordinate()
    pass

#控制关节角度
def controlAngles(key):
    cmd = key.name
    #print(cmd)
    #I
    if(cmd == 'q'):
        val = manipulator.position_angles[0] + 1
        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)
    if(cmd == 'a'):
        val = manipulator.position_angles[0] - 1
        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)

    #T
    if(cmd == 'w'):
        val = manipulator.position_angles[1] + 1
        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)
    if(cmd == 's'):
        val = manipulator.position_angles[1] - 1
        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)

    #T
    if(cmd == 'e'):
        val = manipulator.position_angles[2] + 1
        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)
    if(cmd == 'd'):
        val = manipulator.position_angles[2] - 1
        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)

    #I
    if(cmd == 'r'):
        val = manipulator.position_angles[3] + 1
        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)
    if(cmd == 'f'):
        val = manipulator.position_angles[3] - 1
        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)

    #T
    if(cmd == 't'):
        val = manipulator.position_angles[4] + 1
        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)
    if(cmd == 'g'):
        val = manipulator.position_angles[4] - 1
        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)

    max = 180
    min = -180
    if(manipulator.ids_size <= 6):
        max = 90
        min = 0
     #G/I
    if(cmd == 'y'):      
        val = manipulator.position_angles[5] + 1
        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)
    if(cmd == 'h'):
        val = manipulator.position_angles[5] - 1
        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)

    #G
    if(cmd == 'u'):
        if(manipulator.ids_size > 6):        
           val = manipulator.position_angles[6] + 1
           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)
    if(cmd == 'j'):
        if(manipulator.ids_size > 6):   
           val = manipulator.position_angles[6] - 1
           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)

    manipulator.pushJointsAngles()
    pass


#处理控制键
def handleControlKeys(key):
     cmd = key.name
     #退出监测
     if(cmd == '0'):
        #keyboard.unhook_all()
        return

     if(cmd == 'i' or cmd == 'k' or cmd == 'o' or cmd == 'l' or cmd == 'p' or cmd == 'm' or cmd == 'z' or cmd == 'x' or cmd == 'c' or cmd == 'v' or cmd == 'b' or cmd == 'n'):
        controlCoordinate(key) 

     if(cmd == 'q' or cmd == 'a' or cmd == 'w' or cmd == 's' or cmd == 'e' or cmd == 'd' or cmd == 'r' or cmd == 'f' or cmd == 't' or cmd == 'g' or cmd == 'y' or cmd == 'h' or cmd == 'u' or cmd == 'j'):
        controlAngles(key) 


#-----------------------------------------------------------------------------
#差异代码分隔线
#-----------------------------------------------------------------------------

#动作管理
manipulatorActions = ManipulatorActions(robotlib)

#动作组命令
def handleActionsCmd(key):
    cmd = key.name
    #添加当前姿态到动作列表
    if(cmd == '1'):
        if(manipulatorActions.is_motorsEnable is False):
            manipulatorActions.readMotorsPositions(manipulator)
        manipulatorActions.add(manipulator.position_angles)

    #运行动作列表
    if(cmd == '2'):
        manipulatorActions.run()

    #停止动作
    if(cmd == '3'):
        manipulatorActions.stop()

    #清空当前动作列表
    if(cmd == '4'):
        manipulatorActions.clear()

    #关闭模块使能
    if(cmd == '5'):
        manipulatorActions.enableMotors(False)

    #打开模块使能
    if(cmd == '6'):
        manipulatorActions.enableMotors(True)
    pass


#动作姿态控制
def handleActionControlCmd(key):
    isDown = key.event_type == 'down'
    if not isDown:
        return

    cmd = key.name
    #添加当前姿态到动作列表
    if(cmd == '1' or cmd == '2' or cmd == '3' or cmd == '4' or cmd == '5' or cmd == '6'):
        #if(had_handled_cmd!=cmd):
        #   handleActionsCmd(key)
        handleActionsCmd(key)
    else:
        handleControlKeys(key)
    had_handled_cmd = cmd
    pass


#新增动作组
def controlActionsTips():
    print("Keyboard to control angle: Increase / Reduce")
    print("Joint 1 : Q/A;")
    print("Joint 2 : W/S;")
    print("Joint 3 : E/D;")
    print("Joint 4 : R/F;")
    print("Joint 5 : T/G;")
    print("Joint 6 : Y/H;")
    print("Joint 7 : U/J;")

    print("Coordinate X : I/K;")
    print("Coordinate Y : O/L;")
    print("Coordinate Z : P/M;")
    print("Coordinate Roll: Z/X;")
    print("Coordinate Pitch: C/V;")
    print("Coordinate Yaw: B/N")

    print("")
    print("命令:")
    print("1 :新增姿态")
    print("2 :运行")
    print("3 :停止")
    print("4 :清空当前动作组")
    print("5 :关闭构型模块使能")
    print("6 :开启构型模块使能")
    print("0 :退出")
    #print("-----------------------")
    #print("0 :Exit")
    keyboard.hook(handleActionControlCmd)
    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce
    keyboard.wait('0') # keyboard.wait() 的作用是阻塞进程，直到按下 Ctrl 键。
    keyboard.unhook_all()
    input("输入Enter键继续.....")
    pass

#遥控动作组
def controlActionsSensorControlTips():
    count = len(manipulatorActions.actions_groups)
    print("所有动作组：" + str(count) + "个")

    print("请按红外遥控以控制执行动作组...")
    print("(遥控 1.表示执行动作组一；2.表示执行动作组二.....)")
    print("遥控开关 :退出")

    while True:
        cmd = robotlib.getYaokongSensorCmd()
        if(cmd == 0x13):
            break
        if(cmd > 0):
            num = cmd - 0xA + 1
            b = manipulatorActions.loadGroup(num)
            if b:
               manipulatorActions.run()
        time.sleep(0.01)
        pass
    pass




#主入口命令提示
def mainIndexTips():     
    while True:
      count = len(manipulatorActions.actions_groups)
      print("所有动作组：" + str(count) + "个")
      print("请输入序号以编辑（如：1）")    
      print("-----------------------")
      print("其它命令:")
      print("N :新增动作组")
      print("C :遥控动作组")
      print("E :退出")
      
      cmd = input("请输入：")
      if(cmd == 'N' or cmd == 'n'):
         manipulatorActions.newGroup()
         #count=len(manipulatorActions.actions_groups)
         #print("所有动作组："+str(count)+"个")
         controlActionsTips()

      elif(cmd == 'C' or cmd == 'c'):
         controlActionsSensorControlTips()
      elif(cmd == 'E' or cmd == 'e'):
         break
      else:
         print("尝试打开序号为：" + cmd + "的动作组")
         num = robotlib.converToNum(cmd)
         manipulatorActions.loadGroup(num)
         controlActionsTips()
    pass


#程序开始
mainIndexTips();

